/*
 Xern Entity Relocation Network
 Copyright (C) 2010-2011 Ronie Salgado <roniesalg@gmail.com>

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:

 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.

 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
*/

#ifndef XERN_MATRIX_HPP
#define XERN_MATRIX_HPP

#include <cassert>
#include <cmath>

#include "Vector.hpp"
#include "Quaternion.hpp"

#ifdef XERN_USE_BULLET
#include <LinearMath/btMatrix3x3.h>
#endif

namespace Xern
{

/**
 * Matrix3 - 3x3 matrix
 */
class XERN_ALIGNED(16) Matrix3
{
public:

    // Identity matrix
    Matrix3()
    {
        m1m1 = 1;
        m1m2 = 0;
        m1m3 = 0;
        m2m1 = 0;
        m2m2 = 1;
        m2m3 = 0;
        m3m1 = 0;
        m3m2 = 0;
        m3m3 = 1;
    }

    Matrix3(float cm1m1, float cm1m2, float cm1m3,
            float cm2m1, float cm2m2, float cm2m3,
            float cm3m1, float cm3m2, float cm3m3)
    {
        m1m1 = cm1m1;
        m1m2 = cm1m2;
        m1m3 = cm1m3;
        m2m1 = cm2m1;
        m2m2 = cm2m2;
        m2m3 = cm2m3;
        m3m1 = cm3m1;
        m3m2 = cm3m2;
        m3m3 = cm3m3;
    }

    Matrix3(const Matrix3 &m)
    {
        m1m1 = m.m1m1;
        m1m2 = m.m1m2;
        m1m3 = m.m1m3;
        m2m1 = m.m2m1;
        m2m2 = m.m2m2;
        m2m3 = m.m2m3;
        m3m1 = m.m3m1;
        m3m2 = m.m3m2;
        m3m3 = m.m3m3;
    }

#ifdef XERN_USE_BULLET
    Matrix3(const btMatrix3x3 &m)
    {
        btVector3 r = m.getRow(1);
        m1m1 = r.x();
        m1m2 = r.y();
        m1m2 = r.z();
        r = m.getRow(2);
        m2m1 = r.x();
        m2m2 = r.y();
        m2m2 = r.z();
        r = m.getRow(3);
        m3m1 = r.x();
        m3m2 = r.y();
        m3m2 = r.z();
    }
#endif //XERN_USE_BULLET

    Matrix3 Transpose() const
    {
        return Matrix3(m1m1, m2m1, m3m1,
                       m1m2, m2m2, m3m2,
                       m1m3, m2m3, m3m3);
    }

    float Determinant() const
    {
        return m1m1*(m2m2*m3m3 - m2m3*m3m2) - m1m2*(m2m1*m3m3 - m2m3*m3m1)
               + m1m3*(m2m1*m3m2 - m2m2*m3m1);
    }

    Matrix3 Inverse() const
    {
        float D = Determinant();
        if (D == 0.0f)
            return Matrix3(); // Lets return an identity matrix if we can't invert
        return (1.0f/D) * Matrix3(m3m3*m2m2 - m3m2*m2m3,
                                  -(m3m3*m1m2 - m3m2*m1m2),
                                  m2m3*m1m2 - m2m2*m1m3,

                                  -(m3m3*m2m1 - m3m1*m2m3),
                                  m3m3*m1m1 - m3m1*m1m3,
                                  -(m2m3*m1m1 - m2m1*m1m3),

                                  m3m2*m2m1 - m3m1*m1m3,
                                  -(m3m2*m1m1 - m3m1*m1m2),
                                  m2m2*m1m1 - m2m1*m1m2);

    }

    Quaternion ToQuaternion() const
    {
        float trace = m1m1 + m2m2 + m3m3;
        float s;
        Quaternion q;
        if (trace > 0)
        {
            s = 0.5f/squareroot(trace + 1.0f);
            q.w = 0.25/s;
            q.x = (m3m2 - m2m3)*s;
            q.y = (m1m3 - m3m1)*s;
            q.z = (m2m1 - m1m2)*s;
        }
        else
        {
            if (m1m1 > m2m2 && m1m1 > m3m3)
            {
                s = 2.0f*squareroot(1.0f + m1m1 - m2m2 - m3m3);
                q.w = (m3m2 - m2m3)/s;
                q.x = 0.25*s;
                q.y = (m1m2 + m2m1)/s;
                q.z = (m1m3 + m3m1)/s;
            }
            else if (m2m2 > m3m3)
            {
                s = 2.0f*squareroot(1.0f + m2m2 - m1m1 - m3m3);
                q.w = (m1m3 - m3m1)/s;
                q.x = (m1m2 + m2m1)/s;
                q.y = 0.25f*s;
                q.z = (m2m3 + m3m2)/s;
            }
            else
            {
                s = 2.0f*squareroot(1.0f + m3m3 - m1m1 - m2m2);
                q.w = (m2m1 - m1m2)/s;
                q.x = (m1m3 + m3m1)/s;
                q.y = (m2m3 + m3m2)/s;
                q.z = 0.25f*s;
            }
        }

        return q;
    }

    friend bool operator==(const Matrix3 &m1, const Matrix3 &m2)
    {
        if (m1.m1m1 == m2.m1m1 && m1.m1m2 == m2.m1m2 && m1.m1m3 == m2.m1m3 &&
                m1.m2m1 == m2.m2m1 && m1.m2m2 == m2.m2m2 && m1.m2m3 == m2.m2m3 &&
                m1.m3m1 == m2.m3m1 && m1.m3m2 == m2.m3m2 && m1.m3m3 == m2.m3m3)
            return true;
        return false;
    }

    friend bool operator!=(const Matrix3 &m1, const Matrix3 &m2)
    {
        if (m1.m1m1 != m2.m1m1 || m1.m1m2 != m2.m1m2 || m1.m1m3 != m2.m1m3 ||
                m1.m2m1 != m2.m2m1 || m1.m2m2 != m2.m2m2 || m1.m2m3 != m2.m2m3 ||
                m1.m3m1 != m2.m3m1 || m1.m3m2 != m2.m3m2 || m1.m3m3 != m2.m3m3)
            return true;
        return false;
    }

    friend Matrix3 operator+(const Matrix3 &m1, const Matrix3 &m2)
    {
        return Matrix3(m1.m1m1+m2.m1m1, m1.m1m2+m2.m1m2, m1.m1m3+m2.m1m3,
                       m1.m2m1+m2.m2m1, m1.m2m2+m2.m2m2, m1.m2m3+m2.m2m3,
                       m1.m3m1+m2.m3m1, m1.m3m2+m2.m3m2, m1.m3m3+m2.m3m3);
    }

    friend Matrix3 operator-(const Matrix3 &m1, const Matrix3 &m2)
    {
        return Matrix3(m1.m1m1-m2.m1m1, m1.m1m2-m2.m1m2, m1.m1m3-m2.m1m3,
                       m1.m2m1-m2.m2m1, m1.m2m2-m2.m2m2, m1.m2m3-m2.m2m3,
                       m1.m3m1-m2.m3m1, m1.m3m2-m2.m3m2, m1.m3m3-m2.m3m3);
    }

    friend Matrix3 operator*(const Matrix3 &m, float s)
    {
        return Matrix3(m.m1m1*s, m.m1m2*s, m.m1m3*s,
                       m.m2m1*s, m.m2m2*s, m.m2m3*s,
                       m.m3m1*s, m.m3m2*s, m.m3m3*s);
    }

    friend Matrix3 operator*(float s, const Matrix3 &m)
    {
        return Matrix3(m.m1m1*s, m.m1m2*s, m.m1m3*s,
                       m.m2m1*s, m.m2m2*s, m.m2m3*s,
                       m.m3m1*s, m.m3m2*s, m.m3m3*s);
    }

    friend Matrix3 operator/(const Matrix3 &m, float s)
    {
        return m*(1.0f/s);
    }

    friend Matrix3 operator*(const Matrix3 &m1, const Matrix3 &m2)
    {
        return Matrix3(m1.m1m1*m2.m1m1 + m1.m1m2*m2.m2m1 + m1.m1m3*m2.m3m1,
                       m1.m1m1*m2.m1m2 + m1.m1m2*m2.m2m2 + m1.m1m3*m2.m3m2,
                       m1.m1m1*m2.m1m3 + m1.m1m2*m2.m2m3 + m1.m1m3*m2.m3m3,

                       m1.m2m1*m2.m1m1 + m1.m2m2*m2.m2m1 + m1.m2m3*m2.m3m1,
                       m1.m2m1*m2.m1m2 + m1.m2m2*m2.m2m2 + m1.m2m3*m2.m3m2,
                       m1.m2m1*m2.m1m3 + m1.m2m2*m2.m2m3 + m1.m2m3*m2.m3m3,

                       m1.m3m1*m2.m1m1 + m1.m3m2*m2.m2m1 + m1.m3m3*m2.m3m1,
                       m1.m3m1*m2.m1m2 + m1.m3m2*m2.m2m2 + m1.m3m3*m2.m3m2,
                       m1.m3m1*m2.m1m3 + m1.m3m2*m2.m2m3 + m1.m3m3*m2.m3m3);
    }

    friend Vector3 operator*(const Vector3 &v, const Matrix3 &m)
    {
        return Vector3(v.x*m.m1m1 + v.x*m.m2m1 + v.x*m.m3m1,
                       v.y*m.m1m1 + v.y*m.m2m1 + v.y*m.m3m1,
                       v.z*m.m1m1 + v.z*m.m2m1 + v.z*m.m3m1);
    }

    friend Vector3 operator*(const Matrix3 &m, const Vector3 &v)
    {
        return Vector3(m.m1m1*v.x + m.m1m2*v.y + m.m1m3*v.z,
                       m.m2m1*v.x + m.m2m2*v.y + m.m2m3*v.z,
                       m.m3m1*v.x + m.m3m2*v.y + m.m3m3*v.z);
    }

    operator Quaternion() const
    {
        return ToQuaternion(); // TODO: Implement please
    }

    float& operator[](int idx)
    {
        // Sanity check
        assert(idx >= 0 && idx < 9);
        return mval[idx];
    }

    union
    {
        struct
        {
            float m1m1, m1m2, m1m3, _m1m4;
            float m2m1, m2m2, m2m3, _m2m4;
            float m3m1, m3m2, m3m3, _m3m4;
        };
        float mval[12];
        QuadFloat columns[3];
    };
};

inline Matrix3 XRotMatrix3(float rot)
{
    float c = cosine(rot);
    float s = sine(rot);
    return Matrix3(1.0f, 0.0f, 0.0f,
                   0.0f, c, s,
                   0.0f, -s, c);
}

inline Matrix3 YRotMatrix3(float rot)
{
    float c = cosine(rot);
    float s = sine(rot);
    return Matrix3(c,    0.0f, -s,
                   0.0f, 1.0f, 0.0,
                   s,    0.0f, c);
}

inline Matrix3 ZRotMatrix3(float rot)
{
    float c = cosine(rot);
    float s = sine(rot);
    return Matrix3(c, s, 0.0f,
                   -s, c, 0.0f,
                   0.0f, 0.0f, 1.0f);
}

inline Matrix3 XYZRotMatrix3(float x, float y, float z)
{
    return XRotMatrix3(x) * YRotMatrix3(y) * ZRotMatrix3(z);
}

inline Matrix3 XYZRotMatrix3(const Vector3 &rot)
{
    return XRotMatrix3(rot.x) * YRotMatrix3(rot.y) * ZRotMatrix3(rot.z);
}

inline Matrix3 ScaleMatrix3(float sx, float sy, float sz)
{
    return Matrix3(sx, 0.0f, 0.0f,
                   0.0f, sy, 0.0f,
                   0.0f, 0.0f, sz);
}

inline Matrix3 ScaleMatrix3(const Vector3 &v)
{
    return Matrix3(v.x, 0.0f, 0.0f,
                   0.0f, v.y, 0.0f,
                   0.0f, 0.0f, v.z);
}

inline Matrix3 QRotMatrix3(const Quaternion &q)
{
    // Implementation based on bullet one
    float d = q.Length2();
    float s = 2.0f / d;
    float xs = q.x*s,  ys = q.y*s,  zs = q.z*s;
    float wx = q.w*xs, wy = q.w*ys, wz = q.w*zs;
    float xx = q.x*xs, xy = q.x*ys, xz = q.x*zs;
    float yy = q.y*ys, yz = q.y*zs, zz = q.z*zs;

    return Matrix3(
               1.0f - (yy+zz), xy - wz, xz + wy,
               xy + wz, 1.0 - (xx + zz), yz - wx,
               xz - wy, yz + wx, 1.0f -(xx + yy));
}

/**
 * Matrix4 - 4x4 matrix implementation
 */
class XERN_ALIGNED(16) Matrix4
{
public:

    // Identity matrix
    Matrix4()
    {
        m1m1 = 1;
        m1m2 = 0;
        m1m3 = 0;
        m1m4 = 0;
        m2m1 = 0;
        m2m2 = 1;
        m2m3 = 0;
        m2m4 = 0;
        m3m1 = 0;
        m3m2 = 0;
        m3m3 = 1;
        m3m4 = 0;
        m4m1 = 0;
        m4m2 = 0;
        m4m3 = 0;
        m4m4 = 1;
    }

    Matrix4(float cm1m1, float cm1m2, float cm1m3, float cm1m4,
            float cm2m1, float cm2m2, float cm2m3, float cm2m4,
            float cm3m1, float cm3m2, float cm3m3, float cm3m4,
            float cm4m1, float cm4m2, float cm4m3, float cm4m4)
    {
        m1m1 = cm1m1;
        m1m2 = cm1m2;
        m1m3 = cm1m3;
        m1m4 = cm1m4;
        m2m1 = cm2m1;
        m2m2 = cm2m2;
        m2m3 = cm2m3;
        m2m4 = cm2m4;
        m3m1 = cm3m1;
        m3m2 = cm3m2;
        m3m3 = cm3m3;
        m3m4 = cm3m4;
        m4m1 = cm4m1;
        m4m2 = cm4m2;
        m4m3 = cm4m3;
        m4m4 = cm4m4;
    }

    Matrix4(const Matrix4 &m)
    {
        m1m1 = m.m1m1;
        m1m2 = m.m1m2;
        m1m3 = m.m1m3;
        m1m4 = m.m1m4;
        m2m1 = m.m2m1;
        m2m2 = m.m2m2;
        m2m3 = m.m2m3;
        m2m4 = m.m2m4;
        m3m1 = m.m3m1;
        m3m2 = m.m3m2;
        m3m3 = m.m3m3;
        m3m4 = m.m3m4;
        m4m1 = m.m4m1;
        m4m2 = m.m4m2;
        m4m3 = m.m4m3;
        m4m4 = m.m4m4;
    }

    Matrix4(const Matrix3 &m)
    {
        m1m1 = m.m1m1;
        m1m2 = m.m1m2;
        m1m3 = m.m1m3;
        m1m4 = 0;
        m2m1 = m.m2m1;
        m2m2 = m.m2m2;
        m2m3 = m.m2m3;
        m2m4 = 0;
        m3m1 = m.m3m1;
        m3m2 = m.m3m2;
        m3m3 = m.m3m3;
        m3m4 = 0;
        m4m1 = 0;
        m4m2 = 0;
        m4m3 = 0;
        m4m4 = 1;
    }

    Matrix3 RotationPart() const
    {
        return Matrix3(m1m1, m1m2, m1m3,
                       m2m1, m2m2, m2m3,
                       m3m1, m3m2, m3m3);
    }

    Vector3 TranslationPart() const
    {
        return Vector3(m1m4, m2m4, m3m4);
    }

    Matrix4 Transpose() const
    {
        return Matrix4(m1m1, m2m1, m3m1, m4m1,
                       m1m2, m2m2, m3m2, m4m2,
                       m1m3, m2m3, m3m3, m4m3,
                       m1m4, m2m4, m3m4, m4m4);
    }

    Matrix4 Inverse() const
    {
        // Matrix inverse implementation is taken from Geometrics Tools, LLC
        // implementation licensed with the Boost License 1.0, taken from:
        // http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix4.inl

        float fA0 = mval[ 0]*mval[ 5] - mval[ 1]*mval[ 4];
        float fA1 = mval[ 0]*mval[ 6] - mval[ 2]*mval[ 4];
        float fA2 = mval[ 0]*mval[ 7] - mval[ 3]*mval[ 4];
        float fA3 = mval[ 1]*mval[ 6] - mval[ 2]*mval[ 5];
        float fA4 = mval[ 1]*mval[ 7] - mval[ 3]*mval[ 5];
        float fA5 = mval[ 2]*mval[ 7] - mval[ 3]*mval[ 6];
        float fB0 = mval[ 8]*mval[13] - mval[ 9]*mval[12];
        float fB1 = mval[ 8]*mval[14] - mval[10]*mval[12];
        float fB2 = mval[ 8]*mval[15] - mval[11]*mval[12];
        float fB3 = mval[ 9]*mval[14] - mval[10]*mval[13];
        float fB4 = mval[ 9]*mval[15] - mval[11]*mval[13];
        float fB5 = mval[10]*mval[15] - mval[11]*mval[14];

        float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
        if (-X_EPSILON <= fDet && fDet <= X_EPSILON )
        {
            return Matrix4();
        }

        Matrix4 kInv;
        kInv.mval[ 0] =
            + mval[ 5]*fB5 - mval[ 6]*fB4 + mval[ 7]*fB3;
        kInv.mval[ 4] =
            - mval[ 4]*fB5 + mval[ 6]*fB2 - mval[ 7]*fB1;
        kInv.mval[ 8] =
            + mval[ 4]*fB4 - mval[ 5]*fB2 + mval[ 7]*fB0;
        kInv.mval[12] =
            - mval[ 4]*fB3 + mval[ 5]*fB1 - mval[ 6]*fB0;
        kInv.mval[ 1] =
            - mval[ 1]*fB5 + mval[ 2]*fB4 - mval[ 3]*fB3;
        kInv.mval[ 5] =
            + mval[ 0]*fB5 - mval[ 2]*fB2 + mval[ 3]*fB1;
        kInv.mval[ 9] =
            - mval[ 0]*fB4 + mval[ 1]*fB2 - mval[ 3]*fB0;
        kInv.mval[13] =
            + mval[ 0]*fB3 - mval[ 1]*fB1 + mval[ 2]*fB0;
        kInv.mval[ 2] =
            + mval[13]*fA5 - mval[14]*fA4 + mval[15]*fA3;
        kInv.mval[ 6] =
            - mval[12]*fA5 + mval[14]*fA2 - mval[15]*fA1;
        kInv.mval[10] =
            + mval[12]*fA4 - mval[13]*fA2 + mval[15]*fA0;
        kInv.mval[14] =
            - mval[12]*fA3 + mval[13]*fA1 - mval[14]*fA0;
        kInv.mval[ 3] =
            - mval[ 9]*fA5 + mval[10]*fA4 - mval[11]*fA3;
        kInv.mval[ 7] =
            + mval[ 8]*fA5 - mval[10]*fA2 + mval[11]*fA1;
        kInv.mval[11] =
            - mval[ 8]*fA4 + mval[ 9]*fA2 - mval[11]*fA0;
        kInv.mval[15] =
            + mval[ 8]*fA3 - mval[ 9]*fA1 + mval[10]*fA0;

        float fInvDet = ((float)1.0)/fDet;
        kInv.mval[ 0] *= fInvDet;
        kInv.mval[ 1] *= fInvDet;
        kInv.mval[ 2] *= fInvDet;
        kInv.mval[ 3] *= fInvDet;
        kInv.mval[ 4] *= fInvDet;
        kInv.mval[ 5] *= fInvDet;
        kInv.mval[ 6] *= fInvDet;
        kInv.mval[ 7] *= fInvDet;
        kInv.mval[ 8] *= fInvDet;
        kInv.mval[ 9] *= fInvDet;
        kInv.mval[10] *= fInvDet;
        kInv.mval[11] *= fInvDet;
        kInv.mval[12] *= fInvDet;
        kInv.mval[13] *= fInvDet;
        kInv.mval[14] *= fInvDet;
        kInv.mval[15] *= fInvDet;

        return kInv.Transpose(); // Transpose the resultant matrix.
        // Due to the different storing mode
    }

    /*Matrix4 &operator=(const Matrix4 &m)
    {
      m1m1 = m.m1m1; m1m2 = m.m1m2; m1m3 = m.m1m3; m1m4 = m.m1m4;
      m2m1 = m.m2m1; m2m2 = m.m2m2; m2m3 = m.m2m3; m2m4 = m.m2m4;
      m3m1 = m.m3m1; m3m2 = m.m3m2; m3m3 = m.m3m3; m3m4 = m.m2m4;
      m4m1 = m.m4m1; m4m2 = m.m4m2; m4m3 = m.m4m3; m4m4 = m.m2m4;
      return *this;
    }

    Matrix4 &operator=(const Matrix3 &m)
    {
      m1m1 = m.m1m1; m1m2 = m.m1m2; m1m3 = m.m1m3; m1m4 = 0;
      m2m1 = m.m2m1; m2m2 = m.m2m2; m2m3 = m.m2m3; m2m4 = 0;
      m3m1 = m.m3m1; m3m2 = m.m3m2; m3m3 = m.m3m3; m3m4 = 0;
      m4m1 = 0; m4m2 = 0; m4m3 = 0; m4m4 = 1;
      return *this;
      }*/

    friend bool operator==(const Matrix4 &m1, const Matrix4 &m2)
    {
        for (int i = 0; i < 16; i++)
        {
            if (m1.mval[i] != m2.mval[i])
                return false;
        }
        return true;
    }

    friend bool operator!=(const Matrix4&m1, const Matrix4 &m2)
    {
        for (int i = 0; i < 16; i++)
        {
            if (m1.mval[i] != m2.mval[i])
                return true;
        }
        return false;
    }

    friend Matrix4 operator+(const Matrix4 &m1, const Matrix4 &m2)
    {
        return Matrix4(
                   m1.m1m1+m2.m1m1, m1.m1m2+m2.m1m2, m1.m1m3+m2.m1m3, m1.m1m4+m2.m1m4,
                   m1.m2m1+m2.m2m1, m1.m2m2+m2.m2m2, m1.m2m3+m2.m2m3, m1.m2m4+m2.m2m4,
                   m1.m3m1+m2.m3m1, m1.m3m2+m2.m3m2, m1.m3m3+m2.m3m3, m1.m3m4+m2.m3m4,
                   m1.m4m1+m2.m4m1, m1.m4m2+m2.m4m2, m1.m4m3+m2.m4m3, m1.m4m4+m2.m4m4);
    }

    friend Matrix4 operator-(const Matrix4 &m1, const Matrix4 &m2)
    {
        return Matrix4(
                   m1.m1m1-m2.m1m1, m1.m1m2-m2.m1m2, m1.m1m3-m2.m1m3, m1.m1m4-m2.m1m4,
                   m1.m2m1-m2.m2m1, m1.m2m2-m2.m2m2, m1.m2m3-m2.m2m3, m1.m2m4-m2.m2m4,
                   m1.m3m1-m2.m3m1, m1.m3m2-m2.m3m2, m1.m3m3-m2.m3m3, m1.m3m4-m2.m3m4,
                   m1.m4m1-m2.m4m1, m1.m4m2-m2.m4m2, m1.m4m3-m2.m4m3, m1.m4m4-m2.m4m4);
    }

    friend Matrix4 operator*(const Matrix4 &m, float s)
    {
        return Matrix4(m.m1m1*s, m.m1m2*s, m.m1m3*s, m.m1m4*s,
                       m.m2m1*s, m.m2m2*s, m.m2m3*s, m.m2m4*s,
                       m.m3m1*s, m.m3m2*s, m.m3m3*s, m.m3m4*s,
                       m.m4m1*s, m.m4m2*s, m.m4m3*s, m.m4m4*s);
    }

    friend Matrix4 operator*(float s, const Matrix4 &m)
    {
        return Matrix4(m.m1m1*s, m.m1m2*s, m.m1m3*s, m.m1m4*s,
                       m.m2m1*s, m.m2m2*s, m.m2m3*s, m.m2m4*s,
                       m.m3m1*s, m.m3m2*s, m.m3m3*s, m.m3m4*s,
                       m.m4m1*s, m.m4m2*s, m.m4m3*s, m.m4m4*s);
    }

    friend Matrix4 operator/(Matrix4 &m, float s)
    {
        return m*(1.0f/s);
    }

    friend Matrix4 operator*(const Matrix4 &m1, const Matrix4 &m2)
    {
        return Matrix4(
                   m1.m1m1*m2.m1m1 + m1.m1m2*m2.m2m1 + m1.m1m3*m2.m3m1 + m1.m1m4*m2.m4m1,
                   m1.m1m1*m2.m1m2 + m1.m1m2*m2.m2m2 + m1.m1m3*m2.m3m2 + m1.m1m4*m2.m4m2,
                   m1.m1m1*m2.m1m3 + m1.m1m2*m2.m2m3 + m1.m1m3*m2.m3m3 + m1.m1m4*m2.m4m3,
                   m1.m1m1*m2.m1m4 + m1.m1m2*m2.m2m4 + m1.m1m3*m2.m3m4 + m1.m1m4*m2.m4m4,

                   m1.m2m1*m2.m1m1 + m1.m2m2*m2.m2m1 + m1.m2m3*m2.m3m1 + m1.m2m4*m2.m4m1,
                   m1.m2m1*m2.m1m2 + m1.m2m2*m2.m2m2 + m1.m2m3*m2.m3m2 + m1.m2m4*m2.m4m2,
                   m1.m2m1*m2.m1m3 + m1.m2m2*m2.m2m3 + m1.m2m3*m2.m3m3 + m1.m2m4*m2.m4m3,
                   m1.m2m1*m2.m1m4 + m1.m2m2*m2.m2m4 + m1.m2m3*m2.m3m4 + m1.m2m4*m2.m4m4,

                   m1.m3m1*m2.m1m1 + m1.m3m2*m2.m2m1 + m1.m3m3*m2.m3m1 + m1.m3m4*m2.m4m1,
                   m1.m3m1*m2.m1m2 + m1.m3m2*m2.m2m2 + m1.m3m3*m2.m3m2 + m1.m3m4*m2.m4m2,
                   m1.m3m1*m2.m1m3 + m1.m3m2*m2.m2m3 + m1.m3m3*m2.m3m3 + m1.m3m4*m2.m4m3,
                   m1.m3m1*m2.m1m4 + m1.m3m2*m2.m2m4 + m1.m3m3*m2.m3m4 + m1.m3m4*m2.m4m4,

                   m1.m4m1*m2.m1m1 + m1.m4m2*m2.m2m1 + m1.m4m3*m2.m3m1 + m1.m4m4*m2.m4m1,
                   m1.m4m1*m2.m1m2 + m1.m4m2*m2.m2m2 + m1.m4m3*m2.m3m2 + m1.m4m4*m2.m4m2,
                   m1.m4m1*m2.m1m3 + m1.m4m2*m2.m2m3 + m1.m4m3*m2.m3m3 + m1.m4m4*m2.m4m3,
                   m1.m4m1*m2.m1m4 + m1.m4m2*m2.m2m4 + m1.m4m3*m2.m3m4 + m1.m4m4*m2.m4m4);
    }

    friend Vector3 operator*(const Matrix4 &m, const Vector3 &v)
    {
        return Vector3(m.m1m1*v.x + m.m1m2*v.y + m.m1m3*v.z + m.m1m4,
                       m.m2m1*v.x + m.m2m2*v.y + m.m2m3*v.z + m.m2m4,
                       m.m3m1*v.x + m.m3m2*v.y + m.m3m3*v.z + m.m3m4)
               / (m.m4m1*v.x + m.m4m2*v.y + m.m4m3*v.z + m.m4m4);
    }

    friend Vector3 operator*(const Vector3 &v, const Matrix4 &m)
    {
        return Vector3(v.x*m.m1m1 + v.y*m.m2m1 + v.z*m.m3m1 + m.m4m1,
                       v.x*m.m1m2 + v.y*m.m2m2 + v.z*m.m3m2 + m.m4m2,
                       v.x*m.m1m3 + v.y*m.m2m3 + v.z*m.m3m3 + m.m4m3)
               / (v.x*m.m1m4 + v.y*m.m2m4 + v.z*m.m3m4 + m.m4m4);
    }

    friend SVector4 operator*(const Matrix4 &m, const SVector4 &v)
    {
        return SVector4(m.m1m1*v.x + m.m1m2*v.y + m.m1m3*v.z + m.m1m4,
                        m.m2m1*v.x + m.m2m2*v.y + m.m2m3*v.z + m.m2m4,
                        m.m3m1*v.x + m.m3m2*v.y + m.m3m3*v.z + m.m3m4,
                        m.m4m1*v.x + m.m4m2*v.y + m.m4m3*v.z + m.m4m4);
    }

    friend SVector4 operator*(const SVector4 &v, const Matrix4 &m)
    {
        return SVector4(v.x*m.m1m1 + v.y*m.m2m1 + v.z*m.m3m1 + m.m4m1,
                        v.x*m.m1m2 + v.y*m.m2m2 + v.z*m.m3m2 + m.m4m2,
                        v.x*m.m1m3 + v.y*m.m2m3 + v.z*m.m3m3 + m.m4m3,
                        v.x*m.m1m4 + v.y*m.m2m4 + v.z*m.m3m4 + m.m4m4);
    }

    float& operator[](int idx)
    {
        // Sanity check
        assert(idx >= 0 && idx < 16);
        return mval[idx];
    }

    union
    {
        struct
        {
            float m1m1, m2m1, m3m1, m4m1;
            float m1m2, m2m2, m3m2, m4m2;
            float m1m3, m2m3, m3m3, m4m3;
            float m1m4, m2m4, m3m4, m4m4;
        };
        float mval[16];
        QuadFloat columns[4];
    };
};

inline Matrix4 TranslationMatrix(const Vector3 &v)
{
    return Matrix4(1.0f, 0.0f, 0.0f, v.x,
                   0.0f, 1.0f, 0.0f, v.y,
                   0.0f, 0.0f, 1.0f, v.z,
                   0.0f, 0.0f, 0.0f, 1.0f);
}

inline Matrix4 QVecMatrix4(const Quaternion &q, const Vector3 &v)
{
    // Implementation based on bullet one
    float d = q.Length2();
    float s = 2.0f / d;
    float xs = q.x*s,  ys = q.y*s,  zs = q.z*s;
    float wx = q.w*xs, wy = q.w*ys, wz = q.w*zs;
    float xx = q.x*xs, xy = q.x*ys, xz = q.x*zs;
    float yy = q.y*ys, yz = q.y*zs, zz = q.z*zs;

    return Matrix4(
               1.0f - (yy+zz), xy - wz, xz + wy, v.x,
               xy + wz, 1.0 - (xx + zz), yz - wx, v.y,
               xz - wy, yz + wx, 1.0f -(xx + yy), v.z,
               0.0f, 0.0f, 0.0f, 1.0f);
}

inline std::ostream &operator<<(std::ostream &os, const Matrix3 &m)
{
    os << m.m1m1 << m.m1m2 << m.m1m3 << std::endl;
    os << m.m2m1 << m.m2m2 << m.m2m3 << std::endl;
    os << m.m3m1 << m.m3m2 << m.m3m3 << std::endl;
    return os;
}

inline std::ostream &operator<<(std::ostream &os, const Matrix4 &m)
{
    os << m.m1m1 << m.m1m2 << m.m1m3 << m.m1m4 << std::endl;
    os << m.m2m1 << m.m2m2 << m.m2m3 << m.m2m4 << std::endl;
    os << m.m3m1 << m.m3m2 << m.m3m3 << m.m3m4 << std::endl;
    os << m.m4m1 << m.m4m2 << m.m4m3 << m.m4m4 << std::endl;
    return os;
}

};

#endif //XERN_MATRIX_H

